#include <SimpleFOC.h>


#if 0
/**
 * Magnetic sensor reading analog voltage on pin A1.  (自己的是不是A1引脚呢)
 * This voltage is proportional to rotation position.
 * Tested on AS5600 magnetic sensor running in 'analog mode'.  
 * Note AS5600 works better in 'i2C mode' (less noise) but only supports one sensor per i2c bus. 
 * （上面的这个注释很好）
 * 
 * MagneticSensorAnalog(uint8_t _pinAnalog, int _min, int _max)
 * - pinAnalog      - the pin that is reading the pwm from magnetic sensor
 * - min_raw_count  - the smallest expected reading.  Whilst you might expect it to be 0 it is often ~15.  Getting this wrong results in a small click once per revolution
 * - max_raw_count  - the largest value read.  whilst you might expect it to be 2^10 = 1023 it is often ~ 1020. Note ESP32 will be closer to 4096 with its 12bit ADC
  实在不理解 min max 两个参数的意义，直接1024不就行了吗？为啥还要弄14 和 1020 呢？
 */
//
//MagneticSensorAnalog sensor_analog = MagneticSensorAnalog(A1, 14, 1020);
MagneticSensorAnalog sensor_analog = MagneticSensorAnalog(A7, 14, 1020);  //左边的传感器
//MagneticSensorAnalog sensor_analog = MagneticSensorAnalog(A6, 14, 1020);  //左边的传感器


//MagneticSensorI2C sensor_i2c = MagneticSensorI2C(AS5600_I2C);
//MagneticSensorSPI sensor_spi = MagneticSensorSPI(10,14,0x3FFF/*角度寄存器*/);

void setup() {
  // monitoring port
  Serial.begin(115200);

  // initialise magnetic sensor hardware
  sensor_analog.init();

  Serial.println("Sensor ready");
  _delay(1000);
}

void loop() {
  // iterative function updating the sensor internal variables
  // it is usually called in motor.loopFOC()
  // this function reads the sensor hardware and 
  // has to be called before getAngle nad getVelocity
  sensor_analog.update();
  // display the angle and the angular velocity to the terminal
  Serial.print("Angle:");
  Serial.print(sensor_analog.getAngle());
  Serial.print("\t");
  Serial.print("Velo:");
  Serial.println(sensor_analog.getVelocity());


  //delay(200);不加这行代码，会有迟滞的现象------旋转后数值不立马变化

  //（M1---A6）测试：每转一圈，度数改变6-7度（与IIC方式读取数据相同）
}
#endif 
//IIC method
#if 1

// MagneticSensorI2C(uint8_t _chip_address, float _cpr, uint8_t _angle_register_msb)
//  chip_address  I2C chip address
//  bit_resolution  resolution of the sensor(这里需要明确，如果是MT6701 则是多少呢？)
//  angle_register_msb  angle read register msb（低字节还是高字节在前）
//  bits_used_msb  number of used bits in msb register（什么意思）
// 
// make sure to read the chip address and the chip angle register msb value from the datasheet
// also in most cases you will need external pull-ups on SDA and SCL lines!!!!!
//
// For AS5058B
// MagneticSensorI2C sensor = MagneticSensorI2C(0x40, 14, 0xFE, 8);

// Example of AS5600 configuration 
MagneticSensorI2C sensor_as5600 = MagneticSensorI2C(AS5600_I2C);
MagneticSensorI2C sensor = MagneticSensorI2C(MT6701_I2C);

void setup() {
  // monitoring port
  Serial.begin(115200);

  // configure i2C
  Wire.setClock(400000);
  // initialise magnetic sensor hardware
  sensor.init();
  sensor_as5600.init();

  Serial.println("Sensor ready");
  _delay(1000);
}



void loop() {
  // iterative function updating the sensor internal variables
  // it is usually called in motor.loopFOC()
  // this function reads the sensor hardware and 
  // has to be called before getAngle nad getVelocity
  sensor.update();
  // display the angle and the angular velocity to the terminal
  Serial.print("Angle:");
  Serial.print(sensor.getAngle());
  Serial.print("\t");
  Serial.print("Velo:");
  Serial.println(sensor.getVelocity());



  sensor_as5600.update();
  // display the angle and the angular velocity to the terminal
  Serial.print("AS5600 Angle:");
  Serial.print(sensor_as5600.getAngle());
  Serial.print("\t");
  Serial.print("Velo:");
  Serial.println(sensor_as5600.getVelocity());


  //delay(200);
}


#endif



//SPI method
#if  0
// MagneticSensorSPI(MagneticSensorSPIConfig_s config, int cs)
//  config  - SPI config
//  cs      - SPI chip select pin 

// magnetic sensor instance - SPI
MagneticSensorSPI sensor = MagneticSensorSPI(AS5147_SPI, 10);
// alternative constructor (chipselsect_PIN, bit_resolution, angle_read_register, )
// MagneticSensorSPI sensor = MagneticSensorSPI(10, 14, 0x3FFF);

#endif 

//PWM method
#if 0

/**
 * Magnetic sensor reading pwm signal on pin 2.  
 * The pwm duty cycle is proportional to the sensor angle.
 * 
 * MagneticSensorPWM(uint8_t MagneticSensorPWM, int _min, int _max)
 * - pinPWM         - the pin that is reading the pwm from magnetic sensor
 * - min_raw_count  - the smallest expected reading.  Whilst you might expect it to be 0 it is often ~5.  Getting this wrong results in a small click once per revolution
 * - max_raw_count  - the largest value read.  whilst you might expect it to be 1kHz = 1000 it is often ~910. depending on the exact frequency and saturation
 */
MagneticSensorPWM sensor = MagneticSensorPWM(2, 4, 904);
void doPWM(){sensor.handlePWM();}

void setup() {
  // monitoring port
  Serial.begin(115200);

  // initialise magnetic sensor hardware
  sensor.init();
  // comment out to use sensor in blocking (non-interrupt) way
  sensor.enableInterrupt(doPWM);

  Serial.println("Sensor ready");
  _delay(1000);
}
#endif